
#include "deformation.h"
#include <cassert>

Number Deformation::omega(Number d) {
   
   if (d==NINFINITY) { return 0.00000001; }
   
   if (d==0)  d = 0.00000001;
   return 1.0/(pow(d,WEXPONENT));
   
}

Deformation::Deformation() {
   
   i_vpositions = 0;
   i_constraintsPos = 0;
   d_constraintsPos = 0;
   WeightsTable = 0;
   phatTable = 0;
   
   WEXPONENT = 2.0;
}

Deformation::~Deformation() {
   
   if (WeightsTable != 0)   delete []WeightsTable;
   if (phatTable != 0)   delete []phatTable;
}

void Deformation::setting(const vector<Point3> *ipos, unsigned n, const std::vector<Point3> *icpos) {
   
   assert(ipos!=0 and n>0 and icpos!=0);
   i_vpositions = ipos;
   i_constraintsPos = icpos;
   npositions = n;
   nconstraints = i_constraintsPos->size();
   
   int texSize = (int)(std::ceil(std::sqrt(npositions)));
   texSizetexSize = texSize*texSize;
}

void Deformation::setting(const vector<Point3> *ipos, unsigned n, const vector<Point3> *icpos, Number *W) {
   
   assert(ipos!=0 and n>0 and icpos!=0 and W!=0);
   
   i_vpositions = ipos;
   i_constraintsPos = icpos;
   npositions = n;
   nconstraints = i_constraintsPos->size();
   WeightsTable = W;
   
   int texSize = (int)(std::ceil(std::sqrt(npositions)));
   texSizetexSize = texSize*texSize;
}

void Deformation::clear() {
   
}
   
void Deformation::initialize() {
   
   assert(i_vpositions != 0 and i_constraintsPos->size() != 0);
   assert(npositions > 0);
   assert(nconstraints > 0);
   
   // pre-compute the data
   if (WeightsTable==0) compute_WeightsTable(); 
   compute_pstarList_vhatList(); 
   compute_phatTable(); 
}
   
void Deformation::execute_deformation(const vector<Point3> *dcpos, Number *vpositions) {
   
   d_constraintsPos = dcpos; // positions of deformed constraints 
   for (unsigned iv=0, i=0; i<npositions; iv+=3, ++i) {
      
      compute_qstar_points(i); 
      computeCorrelationMatrix_points(i); 
      eigenvalue = computeLargestRootOfQuarticEquation() + traceM; 
      
      compute_u(); 
      
      // Apply the rotation on the vertex position
      computeRotation(&vhatList[i][0]); 
      float *p = &vpositions[iv];
      p[0]=qstar[0]+rv[0]; p[1]=qstar[1]+rv[1]; p[2]=qstar[2]+rv[2];
   }
}

void Deformation::computeRotation(const Number v[]) {
   
   /// Perform the rotation of v
   vxu[0] = v[1]*u[2]-v[2]*u[1];
   vxu[1] = v[2]*u[0]-v[0]*u[2];
   vxu[2] = v[0]*u[1]-v[1]*u[0];
   
   factor = -2.0/(1.0 + u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
   rv[0] = u[1]*vxu[2] - u[2]*vxu[1] + vxu[0];
   rv[1] = u[2]*vxu[0] - u[0]*vxu[2] + vxu[1];
   rv[2] = u[0]*vxu[1] - u[1]*vxu[0] + vxu[2];
   
   rv[0] = v[0] + factor*rv[0];
   rv[1] = v[1] + factor*rv[1];
   rv[2] = v[2] + factor*rv[2];
}

void Deformation::compute_u() {
   
   /// Find the u value
   Nf0[0]=Mf0[0]+Mf0[0]; Nf0[1]=Mf0[1]+Mf1[0]; Nf0[2]=Mf0[2]+Mf2[0];
   Nf1[0]=Mf1[0]+Mf0[1]; Nf1[1]=Mf1[1]+Mf1[1]; Nf1[2]=Mf1[2]+Mf2[1];
   Nf2[0]=Mf2[0]+Mf0[2]; Nf2[1]=Mf2[1]+Mf1[2]; Nf2[2]=Mf2[2]+Mf2[2];
   
   Nf0[0] -= eigenvalue;
   Nf1[1] -= eigenvalue;
   Nf2[2] -= eigenvalue;
   
   /// Solve the 3x3 system 
   //~ const Number sigma = (Nf0[0]*Nf2[1] - Nf2[0]*Nf0[1])/(Nf0[0]*Nf1[1] - Nf1[0]*Nf0[1]);
   //~ u[2] = (Nf0[0]*V[2]  -Nf2[0]*V[0]  -sigma*(Nf0[0]*V[1] - Nf1[0]*V[0]))/
          //~ (Nf0[0]*Nf2[2]-Nf2[0]*Nf0[2]-sigma*(Nf0[0]*Nf1[2]-Nf1[0]*Nf0[2]));
   //~ u[1] = ((Nf0[0]*V[1] -Nf1[0]*V[0])-(Nf0[0]*Nf1[2]-Nf1[0]*Nf0[2])*u[2])/
          //~ (Nf0[0]*Nf1[1] -Nf1[0]*Nf0[1]);
   //~ u[0] = (V[0]-Nf0[2]*u[2]-Nf0[1]*u[1])/Nf0[0];
   
   /// Solve the 3x3 system 
   int pl=-1, pc=-1;
   if (Nf0[0] == 0) {
      for (int i=1; i<3; ++i) 
         if (Nf0[i] != 0) { pl = 0; pc = i; }
      if (pc == -1) {
         for (int i=0; i<3; ++i) 
            if (Nf1[i] != 0) { pl = 1; pc = i; }
      }
      if (pc == -1) {
         for (int i=0; i<3; ++i) 
            if (Nf2[i] != 0) { pl = 2; pc = i; }
      }
      
      if (pc == -1) u[0] = u[1] = u[2] = 0; return;
      if (pl != 0) {
         Number tmp[4] = {Nf0[0], Nf0[1], Nf0[2], V[0]};
         if (pl == 1) {
            Nf0[0] = Nf1[0]; Nf0[1] = Nf1[1]; Nf0[2] = Nf1[2]; V[0] = V[1];
            Nf1[0] = tmp[0]; Nf1[1] = tmp[1]; Nf1[2] = tmp[2]; V[1] = tmp[3];
         }
         else {
            Nf0[0] = Nf2[0]; Nf0[1] = Nf2[1]; Nf0[2] = Nf2[2]; V[0] = V[2];
            Nf1[0] = tmp[0]; Nf1[1] = tmp[1]; Nf1[2] = tmp[2]; V[2] = tmp[3];
         }
      }
      if (pc != 0) {
         Number tmp[3] = {Nf0[0], Nf1[0], Nf2[0]};
         if (pc == 1) {
            Nf0[0] = Nf0[1]; Nf1[0] = Nf1[1]; Nf2[0] = Nf2[1];
            Nf0[1] = tmp[0]; Nf1[1] = tmp[1]; Nf2[1] = tmp[2];
         }
         else {
            Nf0[0] = Nf0[2]; Nf1[0] = Nf1[2]; Nf2[0] = Nf2[2];
            Nf0[2] = tmp[0]; Nf1[2] = tmp[1]; Nf2[2] = tmp[2];
         }
      }
   }
   
   if ((Nf0[0]*Nf1[1] - Nf1[0]*Nf0[1]) != 0) {
      const Number sigma = (Nf0[0]*Nf2[1] - Nf2[0]*Nf0[1])/(Nf0[0]*Nf1[1] - Nf1[0]*Nf0[1]);
      const Number den = (Nf0[0]*Nf2[2]-Nf2[0]*Nf0[2]-sigma*(Nf0[0]*Nf1[2]-Nf1[0]*Nf0[2]));
      if (den != 0) u[2] = (Nf0[0]*V[2]  -Nf2[0]*V[0]  -sigma*(Nf0[0]*V[1] - Nf1[0]*V[0]))/den;
      else          u[2] = 0;
         
      u[1] = ((Nf0[0]*V[1] -Nf1[0]*V[0])-(Nf0[0]*Nf1[2]-Nf1[0]*Nf0[2])*u[2])/
             (Nf0[0]*Nf1[1] -Nf1[0]*Nf0[1]);
      u[0] = (V[0]-Nf0[2]*u[2]-Nf0[1]*u[1])/Nf0[0];
   }
   else if ((Nf0[0]*Nf1[2] - Nf1[0]*Nf0[2]) != 0) {
      const Number sigma = (Nf0[0]*Nf2[2] - Nf2[0]*Nf0[2])/(Nf0[0]*Nf1[2] - Nf1[0]*Nf0[2]);
      const Number den = (Nf0[0]*Nf2[1]-Nf2[0]*Nf0[1]-sigma*(Nf0[0]*Nf1[1]-Nf1[0]*Nf0[1]));
      if (den != 0) u[1] = (Nf0[0]*V[1]  -Nf2[0]*V[0]  -sigma*(Nf0[0]*V[2] - Nf1[0]*V[0]))/den;
      else          u[1] = 0;
         
      u[2] = ((Nf0[0]*V[2] -Nf1[0]*V[0])-(Nf0[0]*Nf1[1]-Nf1[0]*Nf0[1])*u[1])/
             (Nf0[0]*Nf1[2] -Nf1[0]*Nf0[2]);
      u[0] = (V[0]-Nf0[1]*u[1]-Nf0[2]*u[2])/Nf0[0];
   }
   else if ((Nf0[0]*Nf2[1] - Nf2[0]*Nf0[1]) != 0) {
      u[2] = 0;
      u[1] = (Nf0[0]*V[1] -Nf2[0]*V[0])/(Nf0[0]*Nf2[1] -Nf2[0]*Nf0[1]);
      u[0] = (V[0]-Nf0[1]*u[1])/Nf0[0];
   }
   else if ((Nf0[0]*Nf2[2] - Nf2[0]*Nf0[2]) != 0) {
      u[1] = 0;
      u[2] = (Nf0[0]*V[2] -Nf2[0]*V[0])/(Nf0[0]*Nf2[2] -Nf2[0]*Nf0[2]);
      u[0] = (V[0]-Nf0[2]*u[2])/Nf0[0];
   }
   else {
      u[1] = 0; 
      u[2] = 0;
      u[0] = V[0]/Nf0[0];
   }
   
   if (pc > 0) {
      Number tmp = u[0]; u[0] = u[pc]; u[pc] = tmp;
   }
}

Number Deformation::computeLargestRootOfQuarticEquation() {
   
   /// Build the cubic equation (economic technique)
   Number normaM2=0.0, prodesc2=0.0, prodNorma2=0.0, normaM[3];
   for (int i=0; i<3; i++) {
      normaM[i]=Mf0[i]*Mf0[i]+Mf1[i]*Mf1[i]+Mf2[i]*Mf2[i];
      normaM2 += normaM[i];
   }
   for (int i=0; i<3; i++) {
      int j=(i+1)%3;
      const Number prod_dot = Mf0[i]*Mf0[j]+Mf1[i]*Mf1[j]+Mf2[i]*Mf2[j];
      prodesc2 += prod_dot*prod_dot;
      prodNorma2 += normaM[i]*normaM[j];
   }
   const Number a = -4.0*normaM2;
   const Number b = 16.0*(prodNorma2-prodesc2);
   const Number c = -64.0*detM*detM;
   
   /// Get one cubic root
   const Number p2 = getCubitRoot_X(a,b,c);
   const Number p = sqrt(p2);
   const Number minusp2_plus_4M2 = -p2 + 4.0*normaM2;
   const Number _16detMdivp = (16.0*detM)/p;
   
   /// the largest root
   Number maxroot = max(-p + sqrt(minusp2_plus_4M2 - _16detMdivp), 
                         p + sqrt(minusp2_plus_4M2 + _16detMdivp));
   
   return maxroot*0.5;
}
   
void Deformation::computeCorrelationMatrix_points(unsigned i) {
   
   Mf0[0]=Mf0[1]=Mf0[2]=
   Mf1[0]=Mf1[1]=Mf1[2]=
   Mf2[0]=Mf2[1]=Mf2[2]=0.0;
   const float *pihatV = &phatTable[i*3]; /// pointer to phat of vertex i
   unsigned tsize=3*texSizetexSize;
   
   for (unsigned j=0, row=0; j<nconstraints; ++j, row+=tsize) {
      const float *pihat = &pihatV[row];
      const Point3 &Q = (*d_constraintsPos)[j];
      W = weight(i, j);
      
      qjhat[0] = W*(Q[0]-qstar[0]);
      qjhat[1] = W*(Q[1]-qstar[1]);
      qjhat[2] = W*(Q[2]-qstar[2]);
      
      Mf0[0]+=qjhat[0]*pihat[0]; Mf0[1]+=qjhat[0]*pihat[1]; Mf0[2]+=qjhat[0]*pihat[2];
      Mf1[0]+=qjhat[1]*pihat[0]; Mf1[1]+=qjhat[1]*pihat[1]; Mf1[2]+=qjhat[1]*pihat[2];
      Mf2[0]+=qjhat[2]*pihat[0]; Mf2[1]+=qjhat[2]*pihat[1]; Mf2[2]+=qjhat[2]*pihat[2];
   }
   
   detM = Mf0[0]*(Mf1[1]*Mf2[2]-Mf1[2]*Mf2[1])+
          Mf0[1]*(Mf1[2]*Mf2[0]-Mf1[0]*Mf2[2])+
          Mf0[2]*(Mf1[0]*Mf2[1]-Mf2[0]*Mf1[1]);
   traceM = Mf0[0] + Mf1[1] + Mf2[2];
   
   V[0]=Mf1[2]-Mf2[1]; V[1]=Mf2[0]-Mf0[2]; V[2]=Mf0[1]-Mf1[0];
}
   
void Deformation::compute_pstar_points(unsigned i) {
   
   Wt=pstar[0]=pstar[1]=pstar[2]=0.0;
   for (unsigned j=0; j<nconstraints; ++j) {
      const Point3 &P = (*i_constraintsPos)[j];
      W = weight(i, j);
      Wt += W;
      pstar[0] += W*P[0]; pstar[1] += W*P[1]; pstar[2] += W*P[2];
   }
   pstar[0] /= Wt; pstar[1] /= Wt; pstar[2] /= Wt;
   wtl[i] = Wt;
}

void Deformation::compute_qstar_points(unsigned i) {
   
   Wt=qstar[0]=qstar[1]=qstar[2]=0.0; 
   for (unsigned j=0; j<nconstraints; ++j) { 
      const Point3 &Q = (*d_constraintsPos)[j];
      W = weight(i, j); 
      Wt += W; 
      qstar[0] += W*Q[0]; qstar[1] += W*Q[1]; qstar[2] += W*Q[2];
   }
   qstar[0] /= Wt; qstar[1] /= Wt; qstar[2] /= Wt;
}

void Deformation::compute_WeightsTable() {
   
   if (WeightsTable != 0) delete []WeightsTable;
   WeightsTable = new float[texSizetexSize*nconstraints];
   
   Number v[3];
   for (unsigned j=0; j<nconstraints; ++j) {
      const Point3 &P = (*i_constraintsPos)[j];
      for (unsigned i=0; i<npositions; ++i) {
         const Point3 &p = (*i_vpositions)[i];
         v[0]=p[0]-P[0]; v[1]=p[1]-P[1]; v[2]=p[2]-P[2];
         WeightsTable[j*texSizetexSize + i] = omega(sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]));
      }
   }
}

void Deformation::compute_pstarList_vhatList() {
   
   vhatList.resize(npositions);
   pstarList.resize(npositions);
   qstarList.resize(npositions);
   wtl.resize(npositions);
   
   for (unsigned i=0; i<npositions; ++i) {
      compute_pstar_points(i);
      pstarList[i] = Vector3(pstar[0],pstar[1],pstar[2]);
      qstarList[i] = Vector3(pstar[0],pstar[1],pstar[2]);
      const Point3 &p = (*i_vpositions)[i];
      vhatList[i] = Vector3(p[0]-pstar[0], p[1]-pstar[1], p[2]-pstar[2]);
   }
}

void Deformation::compute_phatTable() {
   
   // Table phatTable has texSize x texSize size
   // beacuse must be send to GPU as a squared texture
   //~ int texSize = (int)(std::ceil(std::sqrt(npositions)));
   //~ texSizetexSize = texSize*texSize;
   phatTable = new float[3*texSizetexSize*nconstraints];
   
   // Cada entrada da tabela representa o par restricao x vertice 
   // Eh feito desta forma para poder mapear facilmente (dentro do shader)  
   // o phat (segundo a restricao j-th) do vertice sendo avaliado 
   // No final, quando o algoritmo em placa seja chamado, esta tabela sera mapeada numa textura3D
   unsigned tsize=3*texSizetexSize;
   for (unsigned i=0, ccc=0; i<npositions; ++i, ccc+=3) {
      float *pihatV = &phatTable[ccc]; /// pointer to phat of vertex i
      for (unsigned j=0, row=0; j<nconstraints; ++j, row+=tsize) {
         float *pihat = &pihatV[row];
         const Number *P = &(*i_constraintsPos)[j][0];
         pihat[0] = P[0]-pstarList[i][0];
         pihat[1] = P[1]-pstarList[i][1];
         pihat[2] = P[2]-pstarList[i][2];
      }
   }
}

void Deformation::rotate(const Point3 &o, const Vector3 &u, Number angle) {
   
   Number cos_a = cos(angle);
   Number sin_a = sin(angle);
   unsigned tsize=3*texSizetexSize;
   for (unsigned i=0, ccc=0; i<npositions; ++i, ccc+=3) {
      
      // pstarList
      Vector3 &p = pstarList[i];
      Vector3 vp(p[0]-o[0],p[1]-o[1],p[2]-o[2]);
      vp = vp*cos_a + cross_product(u,vp)*sin_a + (u*vp)*u*(1.0-cos_a);
      vp = Vector3(o[0],o[1],o[2]) + vp;
      
      // qstarList
      Vector3 &q = qstarList[i];
      Vector3 vq(q[0]-o[0],q[1]-o[1],q[2]-o[2]);
      vq = vq*cos_a + cross_product(u,vq)*sin_a + (u*vq)*u*(1.0-cos_a);
      q = Vector3(o[0],o[1],o[2]) + vq;
      
      // vhatList
      Vector3 &vb = vhatList[i];
      vb = vb*cos_a + cross_product(u,vb)*sin_a + (u*vb)*u*(1.0-cos_a);
      
      // phatTable 
      float *pihatV = &phatTable[ccc]; /// pointer to phat of vertex i
      for (unsigned j=0, row=0; j<nconstraints; ++j, row+=tsize) {
         float *pihat = &pihatV[row];
         Vector3 v(pihat[0], pihat[1], pihat[2]);
         v = v*cos_a + cross_product(u,v)*sin_a + (u*v)*u*(1.0-cos_a);
         pihat[0]=v[0]; pihat[1]=v[1]; pihat[2]=v[2];
      }
   }
}

void Deformation::translate(const Vector3 t) {
   
   // It is not necessary to translate the vectors
   for (unsigned i=0; i<npositions; ++i ) {
      Vector3 &p = pstarList[i];
      Vector3 &q = qstarList[i];
      p = p + t;
      q = q + t;
   }
}

Number Deformation::getCubitRoot_X(const Number a, const Number b, const Number c) {
   
   Number Q = (3.0*b - a*a)*_1DIV9;
   Number R = (9.0*a*b - 27.0*c - 2.0*a*a*a)*_1DIV54;
   Number D = Q*Q*Q + R*R;
   
   if (D>=0) {  /// one real, two complex :: D==0 root reals, at least two are equal 
      Number sqrtD = sqrt(D);
      Number S = sgn(R+sqrtD)*pow(abs(R+sqrtD), _1DIV3);
      Number T = sgn(R-sqrtD)*pow(abs(R-sqrtD), _1DIV3);
      return -a*_1DIV3 + S + T;
   }
   else {   /// all root are real and unequal
      Number theta = acos(R/sqrt(-Q*Q*Q));
      return 2.0*sqrt(-Q)*cos(theta*_1DIV3)-a*_1DIV3;
   }
}

Number Deformation::getCubitRoot_Y(const Number a, const Number b, const Number c) {
   
   Number q = (2*a*a*a - 9*a*b)/27.0 + c;
   Number p = -(a*a)/3.0 + b;
   
   Number modulo = sqrt(-(p*p*p)/27.0 + EPSILON);
   Number temp = -(q*(1-EPSILON))/(2.0*modulo);
   
//   Number disc = (q*q)/4.0 + (p*p*p)/27.0;
   return (sqrt(-p/3.0 + EPSILON))*2.0*cos(acos(temp)/3.0) - a/3.0;
}

Number Deformation::getCubitRoot_Z(const Number a, const Number b, const Number c) {
   
   Number p = b - (a*a)/3.0;
   Number q = c - (2.0*a*a*a-9.0*a*b)/27.0;
   
   Number k = sqrt(4.0*p*p*p + 27.0*q*q);
   Number t =(-3.0*q*SQRT3 + k)/(6.0*SQRT3);
   Number u =( 3.0*q*SQRT3 + k)/(6.0*SQRT3);
   
   Number y = pow(t,_1DIV3) - pow(u,_1DIV3);
   return y-a/3.0;
}
